#include <linux/init.h>
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/types.h>
#include <linux/kprobes.h>
#include <linux/uaccess.h>
#include <linux/sched.h>
#include <asm/ptrace.h>
#include <linux/workqueue.h>
#include <drm/drmP.h>
#include <drm/drm_connector.h>
#include <drm/drm_drv.h>
#include <drm/drm_crtc_helper.h>


#define DRM_MODE_GETCONNECTOR "drm_mode_getconnector"
#define LOONGSON_GET_MODES "loongson_vga_get_modes"
#define POLL_FUNC_NAME "output_poll_execute"

struct connector_desc {
  int first;

  struct drm_connector *pdc;
};


bool *pdrm_kms_helper_poll = NULL;
static int current_index = 0;
static struct drm_device *pgdrm_device = NULL;
static struct drm_mode_get_connector *pgout_resp = NULL;
const struct drm_ioctl_desc *pdrm_ioctls = NULL;

static struct connector_desc connector_list[16] = {
  [0 ... (16 - 1)] = {
    .first = 1,
    .pdc = NULL,
  }
};

static struct connector_desc *lookup_connector(struct drm_connector *pdc)
{
  int i = 0;

  for (i = 0; i < 16; i++) {
    if (connector_list[i].pdc == pdc) {
      return &connector_list[i];
    }
  }
  

  return NULL;
}

static int show_modes(struct drm_connector *pdrm_connector)
{
  struct drm_display_mode *mode = NULL;
  
  list_for_each_entry(mode, &pdrm_connector->modes, head) {
    pr_info("mode->hdisplay:%d, mode->vdisplay:%d, mode->clock:%d, mode->status:%d, mode->type:%d\n", mode->hdisplay, mode->vdisplay, mode->clock, mode->status, mode->type);
    pr_info("KHZ2PICOS(mode):%ld\n", KHZ2PICOS(mode->clock));
  }
  
  return 0;
}

static int show_pmodes(struct drm_connector *pdrm_connector)
{
  struct drm_display_mode *pmode = NULL;
  
  list_for_each_entry(pmode, &pdrm_connector->probed_modes, head) {
    pr_info("pmode->hdisplay:%d, pmode->vdisplay:%d, pmode->clock:%d, pmode->status:%d, pmode->type:%d\n", pmode->hdisplay, pmode->vdisplay, pmode->clock, pmode->status, pmode->type);
    pr_info("KHZ2PICOS(pmode):%ld\n", KHZ2PICOS(pmode->clock));
  }
  
  return 0;
}

static int handler_pre_loongson_get_modes(struct kprobe *p, struct pt_regs *regs)
{
	int ret;
	struct drm_connector *pdrm_connector = NULL;

#ifdef CONFIG_MIPS
  pr_info("pdrm_connector:%p\n", pdrm_connector);
  pr_info("<%s> handler_pre: current_task.name:%s, p->addr = 0x%p, status = 0x%lx\n",
  p->symbol_name, current->comm, p->addr, regs->cp0_status);

  pdrm_connector = (struct drm_connector *)(regs->regs[4]);

  /* force display output */
  pr_info("connector:%p force display output\n", pdrm_connector);
  ret = drm_add_modes_noedid(pdrm_connector, 1920, 1080);
  pr_info("num_modes:%d\n", ret);
  //drm_add_modes_noedid(pdrm_connector, 1024, 768);
  //show_pmode(pdrm_connector);
  
#endif

	return 0;
}


static void handler_post(struct kprobe *p, struct pt_regs *regs,
				unsigned long flags)
{
	
	pr_info("%s---%d\n", __func__, __LINE__);

	return ;
}

static struct kprobe kp_loongson_get_modes = {
	.symbol_name = LOONGSON_GET_MODES,
};


static int entry_handler_drm_mode_getconnector(struct kretprobe_instance *ri, struct pt_regs *regs)
{
  struct kprobe *p = &(ri->rp->kp);
  struct connector_desc *pco_desc = NULL;
	struct drm_device *pdrm_device = NULL;
	struct drm_connector *pdrm_connector = NULL;
  struct drm_mode_get_connector *pout_resp = NULL;

#ifdef CONFIG_MIPS

  pdrm_device = (struct drm_device *)(regs->regs[4]);
  pgdrm_device = pdrm_device;
  pout_resp = (struct drm_mode_get_connector *)(regs->regs[5]);
  pgout_resp = pout_resp;

  pdrm_connector = drm_connector_lookup(pdrm_device, pout_resp->connector_id);
  pr_info("<%s> pdrm_connector:%p, connector_id:%d, count_modes:%d, pout_resp:%p\n", __func__, pdrm_connector, pout_resp->connector_id, pout_resp->count_modes, pout_resp);
  pr_info("driver_feature:0x%x\n", pdrm_device->driver->driver_features);


  /* disable detect DDC in polling way */
  pdrm_device->mode_config.poll_enabled = 0;
  pdrm_device->mode_config.poll_running = 0;

  /* set DRM_FORCE_ON */
  pdrm_connector->force = DRM_FORCE_ON;
  

  pco_desc = lookup_connector(pdrm_connector);
  if (pco_desc == NULL) {
    pco_desc = &connector_list[current_index++];
    pco_desc->pdc = pdrm_connector;

  }

  pr_info("pdrm_device:%p\n", pdrm_device);
  pr_info("{%s} handler_pre: current_task.name:%s, p->addr = 0x%p, status = 0x%lx\n",
  p->symbol_name, current->comm, p->addr, regs->cp0_status);

#endif

	return 0;
}

static int ret_handler_drm_mode_getconnector(struct kretprobe_instance *ri, struct pt_regs *regs)
{
  unsigned long retval = (unsigned long)regs_return_value(regs);
  struct drm_device *pdrm_device = pgdrm_device;
  static int first = 1;
 
#ifdef CONFIG_MIPS

  if (current_index >= 2) {
    if (first == 1) {
      first = 0;
      pr_info("schedule_delayed_work msecs_to_jiffies(10000);%ld, HZ:%d\n", msecs_to_jiffies(10000), HZ);
      schedule_delayed_work(&(pdrm_device->mode_config.output_poll_work), msecs_to_jiffies(10000));
    }
  }
  

#endif

  pr_info("%s return %ld\n", ri->rp->kp.symbol_name, retval);
  pr_info("-----------------------------------------\n\n");

  return 0;
}

static struct kretprobe kretp_dmgc = {
  .entry_handler = entry_handler_drm_mode_getconnector,
  .handler = ret_handler_drm_mode_getconnector,
  .data_size = 0,
  .maxactive = 20,
};

static int entry_handler_output_poll_execute(struct kretprobe_instance *ri, struct pt_regs *regs)
{
	struct work_struct *pwork = NULL;
  struct delayed_work *delayed_work = NULL;
  struct drm_device *pdrm_device = NULL;
  struct kprobe *p = &(ri->rp->kp);


#ifdef CONFIG_MIPS
  pr_info("<%s> handler_pre: current_task.name:%s, p->addr = 0x%p, status = 0x%lx\n",
  p->symbol_name, current->comm, p->addr, regs->cp0_status);

  pwork = (struct work_struct *)(regs->regs[4]);
  pr_info("pwork:%p\n", pwork);
  delayed_work = to_delayed_work(pwork);
  pdrm_device = container_of(delayed_work, struct drm_device, mode_config.output_poll_work);


  pdrm_device->mode_config.delayed_event = true;

#endif

	return 0;
}

static int ret_handler_output_poll_execute(struct kretprobe_instance *ri, struct pt_regs *regs)
{
  pr_info("%s return\n", ri->rp->kp.symbol_name);
  pr_info("-----------------------------------------\n\n");

  return 0;
}

static struct kretprobe kretp_ope = {
  .entry_handler = entry_handler_output_poll_execute,
  .handler = ret_handler_output_poll_execute,
  .data_size = 0,
  .maxactive = 20,
};

static int pc_module_init(void)
{
	int ret;
	pr_info("%s---%d\n", __func__, __LINE__);

  /* 1, global arguments get */
  pdrm_kms_helper_poll = (bool *)kallsyms_lookup_name("drm_kms_helper_poll");
  if (!pdrm_kms_helper_poll) {
    pr_err("lookup drm_kms_helper_poll failed!\n");
    return -1;;
  }
  pr_info("drm_kms_helper_poll:%d, pdrm_kms_helper_poll:%p\n", *pdrm_kms_helper_poll, pdrm_kms_helper_poll);

  /* set drm_kms_helper_poll to false can disable the I2C-EDID detect in poll method */
  *pdrm_kms_helper_poll = false;


  /* 2, kprobe plant */
  /* 2.1, kretprobe plant */
  kretp_dmgc.kp.symbol_name = DRM_MODE_GETCONNECTOR;
  ret = register_kretprobe(&kretp_dmgc);
  if (ret < 0) {
    pr_err("register_kretprobe failed, returned %d\n", ret);
    return ret;
  }
  pr_info("Planted return probe at %s: %p\n",
    kretp_dmgc.kp.symbol_name, kretp_dmgc.kp.addr);

  kretp_ope.kp.symbol_name = POLL_FUNC_NAME;
  ret = register_kretprobe(&kretp_ope);
  if (ret < 0) {
    pr_err("register_kretprobe failed, returned %d\n", ret);
    return ret;
  }
  pr_info("Planted return probe at %s: %p\n",
    kretp_ope.kp.symbol_name, kretp_ope.kp.addr);

  /* 2.2, kprobe plant */
	kp_loongson_get_modes.pre_handler = handler_pre_loongson_get_modes;
	kp_loongson_get_modes.post_handler = handler_post;

	ret = register_kprobe(&kp_loongson_get_modes);
	if (ret < 0) {
		pr_err("register_kprobe failed, ret:%d\n", ret);
		return ret;
	}
	pr_info("Planted kprobe at %p\n", kp_loongson_get_modes.addr);

	return 0;
}

static void pc_module_exit(void)
{
	pr_info("%s---%d\n", __func__, __LINE__);

  unregister_kprobe(&kp_loongson_get_modes);
	pr_info("kprobe at %p unregistered\n", kp_loongson_get_modes.addr);

  unregister_kretprobe(&kretp_ope);
  pr_info("kretprobe at %p unregistered\n", kretp_ope.kp.addr);
  unregister_kretprobe(&kretp_dmgc);
  pr_info("kretprobe at %p unregistered\n", kretp_dmgc.kp.addr);

	
	return ;
}

module_init(pc_module_init);
module_exit(pc_module_exit);

MODULE_LICENSE("GPL");

